#include "HCI.h"

#include <string.h>

  bool pairWiiUsingSync; // True if pairing was done using the Wii SYNC button.
  bool checkRemoteName; // Used to check remote device's name before connecting.
  bool incomingPS4; // True if a PS4 controller is connecting
  uint8_t classOfDevice[3]; // Class of device of last device

  uint8_t hci_state; // Current state of Bluetooth HCI connection
  uint16_t hci_counter; // Counter used for Bluetooth HCI reset loops
  uint16_t hci_num_reset_loops = 0; // This value indicate how many times it should read before trying to reset
  uint16_t hci_event_flag; // HCI flags of received Bluetooth events
  uint8_t inquiry_counter;
		

  
  uint8_t l2capinbuf[BULK_MAXPKTSIZE]; // General purpose buffer for L2CAP in data
  uint8_t l2capoutbuf[14]; // General purpose buffer for L2CAP out data

/* Macros for HCI event flag tests */
#define hci_check_flag(flag) (hci_event_flag & (flag))
#define hci_set_flag(flag) (hci_event_flag |= (flag))
#define hci_clear_flag(flag) (hci_event_flag &= ~(flag))


uint8_t hcibuf[300],aclbuf[500];
 
  /** Use this to see if it is waiting for a incoming connection. */
  bool waitingForConnection;
  /** This is used by the service to know when to store the device information. */
  bool l2capConnectionClaimed;
  /** This is used by the SPP library to claim the current SDP incoming request. */
  bool sdpConnectionClaimed;
  /** This is used by the SPP library to claim the current RFCOMM incoming request. */
  bool rfcommConnectionClaimed;

  /** The name you wish to make the dongle show up as. It is set automatically by the SPP library. */
  const char* btdName = "Fuze game controller";
  /** The pin you wish to make the dongle use for authentication. It is set automatically by the SPP and BTHID library. */
  const char* btdPin = "0000";

  /** The bluetooth dongles Bluetooth address. */
  uint8_t my_bdaddr[6];
  /** HCI handle for the last connection. */
  
  /** Last incoming devices Bluetooth address. */

  /** First 30 chars of last remote name. */
  char remote_name[30];
  /**
   * The supported HCI Version read from the Bluetooth dongle.
   * Used by the PS3BT library to check the HCI Version of the Bluetooth dongle,
   * it should be at least 3 to work properly with the library.
   */
  uint8_t hci_version;

  /** Call this function to pair with a Wiimote */
 
  /** Used to only send the ACL data to the Wiimote. */
  bool connectToWii;
  /** True if a Wiimote is connecting. */
  bool incomingWii;
  /** True when it should pair with a Wiimote. */
  bool pairWithWii;
  /** True if it's the new Wiimote with the Motion Plus Inside or a Wii U Pro Controller. */
  bool motionPlusInside;
  /** True if it's a Wii U Pro Controller. */
  bool wiiUProController;


 
  /** Used to only send the ACL data to the HID device. */
  bool connectToHIDDevice =1;
  /** True if a HID device is connecting. */
  bool incomingHIDDevice =1;
  /** True when it should pair with a device like a mouse or keyboard-> */
  bool pairWithHIDDevice =1;


struct QueueFormat CMDqueue[5];
extern void USART0_Transmit(uint8_t onebyte);
extern void USART0_TransmitAll(struct QueueFormat* qf);
extern UART_HandleTypeDef huart2;

char ctmp[3];


/************************************************************/
/*                    HCI Commands                        */

/************************************************************/


void HCI_SendQueue()
{
	for(uint8_t i=0;i<5;i++)
	{
		if(CMDqueue[i].NeedSended==true)
		{
			
			USART0_TransmitAll(&CMDqueue[i]);
			
			
			CMDqueue[i].NeedSended=false;
		}
	}
}

void HCI_Command(enum HCICommand hcicommd,uint8_t *data, uint8_t nbytes) {
	hci_clear_flag(HCI_FLAG_CMD_COMPLETE);
	//USART0_Transmit(0x01);  //HCI UART Event header
	
	data[0] = (uint8_t)(((uint16_t)hcicommd >> 0) & 0xFF);
  data[1] = (uint8_t)(((uint16_t)hcicommd >> 8) & 0xFF);
	data[2]=nbytes-3;
	#ifdef usart2_log
	HAL_UART_Transmit(&huart2,(uint8_t*)"\rcommand:",9,500);
	for(uint8_t i = 0; i < nbytes; i++){
		//USART0_Transmit(data[i]);
				sprintf(ctmp,"%02x,",data[i]);
			HAL_UART_Transmit(&huart2, (uint8_t*)ctmp,3,100);
	}
	HAL_UART_Transmit(&huart2, (uint8_t*)"\r",1,100);
	#endif
	for(uint8_t i=0;i<5;i++)
	{
	 if (CMDqueue[i].NeedSended==false)
	 {
		 memcpy(CMDqueue[i].data,data,nbytes);
		 CMDqueue[i].length=nbytes;
		 CMDqueue[i].NeedSended=true;
		 CMDqueue[i].commandtype=0x01;
	    break;
	 }
	}
	
}


void hci_reset() {
        hci_event_flag = 0; // Clear all the flags
//        hcibuf[0] = 0x03; // HCI OCF = 3
//        hcibuf[1] = 0x03 << 2; // HCI OGF = 3
//        hcibuf[2] = 0x00;

        HCI_Command(HCI_Reset,hcibuf, 3);
}

void hci_write_scan_enable() {
        hci_clear_flag(HCI_FLAG_INCOMING_REQUEST);
//        hcibuf[0] = 0x1A; // HCI OCF = 1A
//        hcibuf[1] = 0x03 << 2; // HCI OGF = 3
//        hcibuf[2] = 0x01; // parameter length = 1
   
                hcibuf[3] = 0x03; // Inquiry Scan disabled. Page Scan enabled.

        HCI_Command(HCI_Write_Scan_Enable,hcibuf, 4);
}




void hci_read_bdaddr() {
        hci_clear_flag(HCI_FLAG_READ_BDADDR);
//        hcibuf[0] = 0x09; // HCI OCF = 9
//        hcibuf[1] = 0x04 << 2; // HCI OGF = 4
//        hcibuf[2] = 0x00;

        HCI_Command(HCI_Read_BD_ADDR,hcibuf, 3);
}

void hci_read_local_version_information() {
        hci_clear_flag(HCI_FLAG_READ_VERSION);
//        hcibuf[0] = 0x01; // HCI OCF = 1
//        hcibuf[1] = 0x04 << 2; // HCI OGF = 4
//        hcibuf[2] = 0x00;

        HCI_Command(HCI_Read_Local_Version_Info,hcibuf, 3);
}

void hci_accept_connection(struct RemoteDevice* rd) {
        hci_clear_flag(HCI_FLAG_CONNECT_COMPLETE);
//        hcibuf[0] = 0x09; // HCI OCF = 9
//        hcibuf[1] = 0x01 << 2; // HCI OGF = 1
//        hcibuf[2] = 0x07; // parameter length 7
        hcibuf[3] = rd->disc_bdaddr[0]; // 6 octet bdaddr
        hcibuf[4] = rd->disc_bdaddr[1];
        hcibuf[5] = rd->disc_bdaddr[2];
        hcibuf[6] = rd->disc_bdaddr[3];
        hcibuf[7] = rd->disc_bdaddr[4];
        hcibuf[8] = rd->disc_bdaddr[5];
        hcibuf[9] = 0x00; // Switch role to master

        HCI_Command(HCI_Accept_Connection_Request,hcibuf, 10);
}
void hci_reject_connection_request(uint8_t* tmp_bdaddr)
        {
           

            hcibuf[3] = tmp_bdaddr[0];
            hcibuf[4] = tmp_bdaddr[1];
            hcibuf[5] = tmp_bdaddr[2];
            hcibuf[6] = tmp_bdaddr[3];
            hcibuf[7] = tmp_bdaddr[4];
            hcibuf[8] = tmp_bdaddr[5];
            hcibuf[9] = 0x01;

             HCI_Command(HCI_Reject_Connection_Request, hcibuf,10);
        }


void hci_remote_name(struct RemoteDevice* rd) {
        hci_clear_flag(HCI_FLAG_REMOTE_NAME_COMPLETE);
//        hcibuf[0] = 0x19; // HCI OCF = 19
//        hcibuf[1] = 0x01 << 2; // HCI OGF = 1
//        hcibuf[2] = 0x0A; // parameter length = 10
        hcibuf[3] = rd->disc_bdaddr[0]; // 6 octet bdaddr
        hcibuf[4] = rd->disc_bdaddr[1];
        hcibuf[5] = rd->disc_bdaddr[2];
        hcibuf[6] = rd->disc_bdaddr[3];
        hcibuf[7] = rd->disc_bdaddr[4];
        hcibuf[8] = rd->disc_bdaddr[5];
        hcibuf[9] = 0x01; // Page Scan Repetition Mode
        hcibuf[10] = 0x00; // Reserved
        hcibuf[11] = 0x00; // Clock offset - low byte
        hcibuf[12] = 0x00; // Clock offset - high byte

        HCI_Command(HCI_Remote_Name_Request,hcibuf, 13);
}

void hci_set_local_name(const char* name) {
//        hcibuf[0] = 0x13; // HCI OCF = 13
//        hcibuf[1] = 0x03 << 2; // HCI OGF = 3
//        hcibuf[2] = strlen(name) + 1; // parameter length = the length of the string + end byte
        uint8_t i;
        for(i = 0; i < strlen(name); i++)
                hcibuf[i + 3] = name[i];
        hcibuf[i + 3] = 0x00; // End of string

        HCI_Command(HCI_Write_Local_Name,hcibuf, 4 + strlen(name));
}

void hci_inquiry() {
        hci_clear_flag(HCI_FLAG_DEVICE_FOUND);
//        hcibuf[0] = 0x01;
//        hcibuf[1] = 0x01 << 2; // HCI OGF = 1
//        hcibuf[2] = 0x05; // Parameter Total Length = 5
        hcibuf[3] = 0x33; // LAP: Genera/Unlimited Inquiry Access Code (GIAC = 0x9E8B33) - see https://www.bluetooth.org/Technical/AssignedNumbers/baseband.htm
        hcibuf[4] = 0x8B;
        hcibuf[5] = 0x9E;
        hcibuf[6] = 0x0a; // Inquiry time = 61.44 sec (maximum)
        hcibuf[7] = 0x05; // 10 number of responses

        HCI_Command(HCI_Inquiry,hcibuf, 8);
}

void hci_inquiry_cancel() {
//        hcibuf[0] = 0x02;
//        hcibuf[1] = 0x01 << 2; // HCI OGF = 1
//        hcibuf[2] = 0x00; // Parameter Total Length = 0

        HCI_Command(HCI_Inquiry_Cancel,hcibuf, 3);
}



void hci_connect(struct RemoteDevice* rd) {
        hci_clear_flag(HCI_FLAG_CONNECT_COMPLETE | HCI_FLAG_CONNECT_EVENT);
//        hcibuf[0] = 0x05;
//        hcibuf[1] = 0x01 << 2; // HCI OGF = 1
//        hcibuf[2] = 0x0D; // parameter Total Length = 13
        hcibuf[3] = rd->disc_bdaddr[0]; // 6 octet bdaddr (LSB)
        hcibuf[4] = rd->disc_bdaddr[1];
        hcibuf[5] = rd->disc_bdaddr[2];
        hcibuf[6] = rd->disc_bdaddr[3];
        hcibuf[7] = rd->disc_bdaddr[4];
        hcibuf[8] = rd->disc_bdaddr[5];
        hcibuf[9] = 0x18; // DM1 or DH1 may be used
        hcibuf[10] = 0xCC; // DM3, DH3, DM5, DH5 may be used
        hcibuf[11] = 0x01; // Page repetition mode R1
        hcibuf[12] = 0x00; // Reserved
        hcibuf[13] = rd->disc_offset[0]; // Clock offset
        hcibuf[14] = rd->disc_offset[1]| 0x80; // Invalid clock offset
        hcibuf[15] = 0x01; // Do not allow role switch

        HCI_Command(HCI_Create_Connection,hcibuf, 16);
}





void hci_pin_code_request_reply(struct RemoteDevice* rd) {
//        hcibuf[0] = 0x0D; // HCI OCF = 0D
//        hcibuf[1] = 0x01 << 2; // HCI OGF = 1
//        hcibuf[2] = 0x17; // parameter length 23
        hcibuf[3] = rd->disc_bdaddr[0]; // 6 octet bdaddr
        hcibuf[4] = rd->disc_bdaddr[1];
        hcibuf[5] = rd->disc_bdaddr[2];
        hcibuf[6] = rd->disc_bdaddr[3];
        hcibuf[7] = rd->disc_bdaddr[4];
        hcibuf[8] = rd->disc_bdaddr[5];

                hcibuf[9] = strlen(btdPin); // Length of pin
                uint8_t i;
                for(i = 0; i < strlen(btdPin); i++) // The maximum size of the pin is 16
                        hcibuf[i + 10] = btdPin[i];
                for(; i < 16; i++)
                        hcibuf[i + 10] = 0x00; // The rest should be 0
        

        HCI_Command(HCI_PIN_Code_Request_Reply,hcibuf, 26);
}


void hci_pin_code_negative_request_reply(struct RemoteDevice* rd) {
//        hcibuf[0] = 0x0E; // HCI OCF = 0E
//        hcibuf[1] = 0x01 << 2; // HCI OGF = 1
//        hcibuf[2] = 0x06; // parameter length 6
        hcibuf[3] = rd->disc_bdaddr[0]; // 6 octet bdaddr
        hcibuf[4] = rd->disc_bdaddr[1];
        hcibuf[5] = rd->disc_bdaddr[2];
        hcibuf[6] = rd->disc_bdaddr[3];
        hcibuf[7] = rd->disc_bdaddr[4];
        hcibuf[8] = rd->disc_bdaddr[5];

        HCI_Command(HCI_PIN_Code_Request_Negative_Reply,hcibuf, 9);
}


void hci_link_key_request_negative_reply(struct RemoteDevice* rd) {
//        hcibuf[0] = 0x0C; // HCI OCF = 0C
//        hcibuf[1] = 0x01 << 2; // HCI OGF = 1
//        hcibuf[2] = 0x06; // parameter length 6
        hcibuf[3] = rd->disc_bdaddr[0]; // 6 octet bdaddr
        hcibuf[4] = rd->disc_bdaddr[1];
        hcibuf[5] = rd->disc_bdaddr[2];
        hcibuf[6] = rd->disc_bdaddr[3];
        hcibuf[7] = rd->disc_bdaddr[4];
        hcibuf[8] = rd->disc_bdaddr[5];

        HCI_Command(HCI_Link_Key_Request_Negative_Reply,hcibuf, 9);
}


void hci_authentication_request(struct RemoteDevice* rd) {
//        hcibuf[0] = 0x11; // HCI OCF = 11
//        hcibuf[1] = 0x01 << 2; // HCI OGF = 1
//        hcibuf[2] = 0x02; // parameter length = 2
        hcibuf[3] = (rd->hci_handle[0] ); //connection handle - low byte
        hcibuf[4] = (rd->hci_handle[1] ); //connection handle - high byte

        HCI_Command(HCI_Authentication_Requested,hcibuf, 5);
}

void hci_disconnect(struct RemoteDevice* rd) { // This is called by the different services
        hci_clear_flag(HCI_FLAG_DISCONNECT_COMPLETE);
//        hcibuf[0] = 0x06; // HCI OCF = 6
//        hcibuf[1] = 0x01 << 2; // HCI OGF = 1
//        hcibuf[2] = 0x03; // parameter length = 3
        hcibuf[3] = rd->hci_handle[0]; //connection handle - low byte
        hcibuf[4] =  rd->hci_handle[1]; //connection handle - high byte
        hcibuf[5] = 0x13; // reason

        HCI_Command(HCI_Disconnect,hcibuf, 6);
}



void hci_write_extended_inquiry_response() { // See http://bluetooth-pentest.narod.ru/software/bluetooth_class_of_device-service_generator.html
//        hcibuf[0] = 0x52; // HCI OCF = 52
//        hcibuf[1] = 0x03 << 2; // HCI OGF = 3
//        hcibuf[2] = 12;
	
	hcibuf[3] = 0x00;
  hcibuf[4] = 0x08;
  hcibuf[5] = 0x09;
  hcibuf[6] = 0x45;
  hcibuf[7] = 0x4E;
  hcibuf[8] = 0x54;
  hcibuf[9] = 0x52;
  hcibuf[10] = 0x4F;
  hcibuf[11] = 0x50;
  hcibuf[12] = 0x59;
  hcibuf[13] = 0x02;
  hcibuf[14] = 0x0A;

        HCI_Command(HCI_Write_Extended_Inquiry_Response,hcibuf, 15);
}

void hci_change_packet_type() {
//	hcibuf[0] = 0x0f; // HCI OCF = 0F
//	hcibuf[1] = 0x01 << 2; // HCI OGF = 1
//	hcibuf[2] = 0x04; // parameter length 4
	hcibuf[3] = 0x0c;
	hcibuf[4] = 0x00;
	hcibuf[5] = 0x18;
	hcibuf[6] = 0xcc;
	HCI_Command(Change_Connection_Packet_Type,hcibuf, 7);
}

void hci_read_remote_version(){
//	 hcibuf[0] = 0x1d; // HCI OCF = 1D
//	 hcibuf[1] = 0x01 << 2; // HCI OGF = 3
//	 hcibuf[2] = 0x02; // parameter length = 3
	 hcibuf[3] = 0x0c; 
	 hcibuf[4] = 0x00; 

	 HCI_Command(HCI_Read_Remote_Version,hcibuf, 5);
}

void hci_write_link_policy(struct RemoteDevice* rd){
//	hcibuf[0] = 0x0d; // HCI OCF = 0D
//	hcibuf[1] = 0x02 << 2; // HCI OGF = 1
//	hcibuf[2] = 0x04; // parameter length 4
	hcibuf[3] = rd->hci_handle[0];
	hcibuf[4] = rd->hci_handle[1];
	hcibuf[5] = 0x07;
	hcibuf[6] = 0x00;
	HCI_Command(HCI_Write_Link_Policy_Settings,hcibuf, 7);
}

void hci_write_link_timeout(struct RemoteDevice* rd){
//	hcibuf[0] = 0x37; // HCI OCF = 37
//	hcibuf[1] = 0x03 << 2; // HCI OGF = 1
//	hcibuf[2] = 0x04; // parameter length 4
	hcibuf[3] = rd->hci_handle[0];
	hcibuf[4] = rd->hci_handle[1];
	hcibuf[5] = 0x40;
	hcibuf[6] = 0x1f;
	HCI_Command(HCI_Write_Link_Key_Timeout,hcibuf, 7);
}

void hci_write_authentication_enable(){
//	hcibuf[0] = 0x20; // HCI OCF = 20
//	hcibuf[1] = 0x03 << 2; // HCI OGF = 3
//	hcibuf[2] = 0x01; // parameter length = 1
	hcibuf[3] = 0x00;

	HCI_Command(HCI_Write_Authentication_Enable,hcibuf, 4);
}

void hci_read_buffer_size()
{
//  hcibuf[0] = 0x05; // HCI OCF = 20
//	hcibuf[1] = 0x04 << 2; // HCI OGF = 3
//	hcibuf[2] = 0x00; // parameter length = 1	
 HCI_Command(HCI_Read_Buffer_Size,hcibuf, 3);
}
	void hci_delete_stored_link_key(struct RemoteDevice* rd){
		
	      hcibuf[3] = rd->disc_bdaddr[0]; // 6 octet bdaddr
        hcibuf[4] = rd->disc_bdaddr[1];
        hcibuf[5] = rd->disc_bdaddr[2];
        hcibuf[6] = rd->disc_bdaddr[3];
        hcibuf[7] = rd->disc_bdaddr[4];
        hcibuf[8] = rd->disc_bdaddr[5];
				hcibuf[9] = 0x00;
        HCI_Command(HCI_Delete_Stored_Link_Key,hcibuf, 10);
	
	}
void hci_link_key_request_reply(struct RemoteDevice* rd)
{
        hcibuf[3] = rd->disc_bdaddr[0]; // 6 octet bdaddr
        hcibuf[4] = rd->disc_bdaddr[1];
        hcibuf[5] = rd->disc_bdaddr[2];
        hcibuf[6] = rd->disc_bdaddr[3];
        hcibuf[7] = rd->disc_bdaddr[4];
        hcibuf[8] = rd->disc_bdaddr[5];
	
				hcibuf[9] = rd->disc_linkkey[0];
				hcibuf[10] = rd->disc_linkkey[1];
				hcibuf[11] = rd->disc_linkkey[2];
				hcibuf[12] = rd->disc_linkkey[3];
				hcibuf[13] = rd->disc_linkkey[4];
				hcibuf[14] = rd->disc_linkkey[5];
				hcibuf[15] = rd->disc_linkkey[6];
				hcibuf[16] = rd->disc_linkkey[7];
				hcibuf[17] = rd->disc_linkkey[8];
				hcibuf[18] = rd->disc_linkkey[9];
				hcibuf[19] = rd->disc_linkkey[10];
				hcibuf[20] = rd->disc_linkkey[11];
	hcibuf[21] = rd->disc_linkkey[12];
	hcibuf[22] = rd->disc_linkkey[13];
	hcibuf[23] = rd->disc_linkkey[14];
	hcibuf[24] = rd->disc_linkkey[15];
	
        HCI_Command(HCI_Link_Key_Request_Reply,hcibuf, 25);
}
void hci_io_capability_request_reply(struct RemoteDevice* rd)
{
 hcibuf[3] = rd->disc_bdaddr[0];
 hcibuf[4] = rd->disc_bdaddr[1];
 hcibuf[5] = rd->disc_bdaddr[2];
 hcibuf[6] = rd->disc_bdaddr[3];
 hcibuf[7] = rd->disc_bdaddr[4];
 hcibuf[8] = rd->disc_bdaddr[5];
 hcibuf[9] = 0x01;
 hcibuf[10] = 0x00;
 hcibuf[11] = 0x03;

  HCI_Command(HCI_IO_Capability_Request_Reply,hcibuf, 12);
}
void 	 hci_user_confirmation_request_reply(struct RemoteDevice* rd)
{
 hcibuf[3] = rd->disc_bdaddr[0];
 hcibuf[4] = rd->disc_bdaddr[1];
 hcibuf[5] = rd->disc_bdaddr[2];
 hcibuf[6] = rd->disc_bdaddr[3];
 hcibuf[7] = rd->disc_bdaddr[4];
 hcibuf[8] = rd->disc_bdaddr[5];

  HCI_Command(HCI_User_Confirmation_Request_Reply, hcibuf,9);
}
void hci_sniff_subrating(struct RemoteDevice* rd)
 {
    

    
     hcibuf[3]=rd->hci_handle[0];
	   hcibuf[4]=rd->hci_handle[1];
     hcibuf[5] = 0x18;
     hcibuf[6] = 0x03;
     hcibuf[7] = 0x00;
     hcibuf[8] = 0x00;
     hcibuf[9] = 0x02;
     hcibuf[10] = 0x00;


     HCI_Command(HCI_Sniff_Subrating, hcibuf,11);
 }
void hci_write_simple_paring_mode()
{
 hcibuf[3] = 0x00;
  HCI_Command(HCI_Write_Simple_Pairing_Mode, hcibuf,4);
}
void hci_write_simple_paring_debug_mode()
{ 
	hcibuf[3] = 0x00;
 HCI_Command(HCI_Write_Simple_Pairing_Debug_Mode, hcibuf,4);
}
void hci_set_event_mask()
{
// 00 25 5F FF FF FF FF FF
hcibuf[3] = 0xFF;
hcibuf[4] = 0xFF;
hcibuf[5] = 0xFF;
hcibuf[6] = 0xFF;
hcibuf[7] = 0xFF;
hcibuf[8] = 0x5F; // 0xFF;
hcibuf[9] = 0x25; // 0xBF;
hcibuf[10] = 0x00; // 0x3D;

 HCI_Command(HCI_Set_Event_Mask, hcibuf,11);
}

void hci_write_page_timeout()
{
 hcibuf[3] = 0x00;
 hcibuf[4] = 0x20;

  HCI_Command(HCI_Write_Page_Timeout, hcibuf,5);
}

void hci_write_page_scan_activity()
{
 hcibuf[3] = 0x00;
 hcibuf[4] = 0x04;
 hcibuf[5] = 0x12;
 hcibuf[6] = 0x00;

  HCI_Command(HCI_Write_Page_Scan_Activity, hcibuf,7);
}

void hci_write_page_scan_type()
{
  hcibuf[3] = 0x01;

   HCI_Command(HCI_Write_Page_Scan_Type, hcibuf,4);
}

void hci_write_inquiry_scan_activity()
{
  hcibuf[3] = 0x00;
  hcibuf[4] = 0x08;
  hcibuf[5] = 0x12;
  hcibuf[6] = 0x00;

   HCI_Command(HCI_Write_Inquiry_Scan_Activity, hcibuf,7);
}
void hci_write_inquiry_scan_type()
{
hcibuf[3] = 0x01;

 HCI_Command(HCI_Write_Inquiry_Scan_Type, hcibuf,4);
}
void hci_write_inquiry_mode()
{

            hcibuf[3] = 0x00;

             HCI_Command(HCI_Write_Inquiry_Mode, hcibuf,4);
}
void hci_write_class_of_device()
{
   hcibuf[3] = 0x04;
   hcibuf[4] = 0x02;
   hcibuf[5] = 0x3E;

    HCI_Command(HCI_Write_Class_of_Device, hcibuf,6);
}

void hci_write_local_name()
{
   hcibuf[3] = 0x45;
   hcibuf[4] = 0x4E;
   hcibuf[5] = 0x54;
   hcibuf[6] = 0x52;
   hcibuf[7] = 0x4F;
   hcibuf[8] = 0x50;
   hcibuf[9] = 0x59;

    HCI_Command(HCI_Write_Local_Name, hcibuf,10);
}

/*******************************************************************
 *                                                                 *
 *                        HCI ACL Data Packet                      *
 *                                                                 *
 *   buf[0]          buf[1]          buf[2]          buf[3]
 *   0       4       8    11 12      16              24            31 MSB
 *  .-+-+-+-+-+-+-+-|-+-+-+-|-+-|-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-.
 *  |      HCI Handle       |PB |BC |       Data Total Length       |   HCI ACL Data Packet
 *  .-+-+-+-+-+-+-+-|-+-+-+-|-+-|-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-.
 *
 *   buf[4]          buf[5]          buf[6]          buf[7]
 *   0               8               16                            31 MSB
 *  .-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-.
 *  |            Length             |          Channel ID           |   Basic L2CAP header
 *  .-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-.
 *
 *   buf[8]          buf[9]          buf[10]         buf[11]
 *   0               8               16                            31 MSB
 *  .-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-.
 *  |     Code      |  Identifier   |            Length             |   Control frame (C-frame)
 *  .-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-.   (signaling packet format)
 */
/************************************************************/
/*                    L2CAP Commands                        */

/************************************************************/
void L2CAP_Command(struct RemoteDevice* rd, uint8_t* data, uint8_t nbytes, uint8_t channelLow, uint8_t channelHigh) {
        uint8_t buf[8 + nbytes];
        buf[0] = rd->hci_handle[0]; // HCI handle with PB,BC flag
        buf[1] = rd->hci_handle[1]| 0x20;
        buf[2] = (uint8_t)((4 + nbytes) & 0xff); // HCI ACL total data length
        buf[3] = (uint8_t)((4 + nbytes) >> 8);
        buf[4] = (uint8_t)(nbytes & 0xff); // L2CAP header: Length
        buf[5] = (uint8_t)(nbytes >> 8);
        buf[6] = channelLow;
        buf[7] = channelHigh;

        for(uint16_t i = 0; i < nbytes; i++) // L2CAP C-frame
                buf[8 + i] = data[i];
	#ifdef usart2_log
			HAL_UART_Transmit(&huart2,(uint8_t*)"\rl2capcommand:",14,500);
		
		HAL_UART_Transmit(&huart2,(uint8_t*)"02,",3,500);
	
		//USART0_Transmit(0x02);	//HCI ACL Data Header

		for(uint16_t i = 0; i < (nbytes + 8); i++){
			//USART0_Transmit(buf[i]);
			sprintf(ctmp,"%02x,",buf[i]);
		HAL_UART_Transmit(&huart2,(uint8_t*)ctmp,3,500);
	
		}
		#endif
		for(uint8_t i=0;i<5;i++)
	{
	 if (CMDqueue[i].NeedSended==false)
	 {
		 memcpy(CMDqueue[i].data,buf,nbytes+8);
		 CMDqueue[i].length=nbytes+8;
		 CMDqueue[i].NeedSended=true;
		 CMDqueue[i].commandtype=0x02;
	    break;
	 }
	}
#ifdef usart2_log
		HAL_UART_Transmit(&huart2,(uint8_t*)"\r",1,500);
		#endif
	
#ifdef DEBUG_USB_HOST
                Notify(PSTR("\r\nError sending L2CAP message: 0x"), 0x80);
                Notify(PSTR(" - Channel ID: "), 0x80);
                D_PrintHex<uint8_t > (channelHigh, 0x80);
                Notify(PSTR(" "), 0x80);
                D_PrintHex<uint8_t > (channelLow, 0x80);
#endif
        
}

void l2cap_connection_request(struct RemoteDevice* rd, uint8_t rxid, uint8_t* scid, uint16_t psm) {
        l2capoutbuf[0] = L2CAP_CMD_CONNECTION_REQUEST; // Code
        l2capoutbuf[1] = rxid; // Identifier
        l2capoutbuf[2] = 0x04; // Length
        l2capoutbuf[3] = 0x00;
        l2capoutbuf[4] = (uint8_t)(psm & 0xff); // PSM
        l2capoutbuf[5] = (uint8_t)(psm >> 8);
        l2capoutbuf[6] = scid[0]; // Source CID
        l2capoutbuf[7] = scid[1];

        L2CAP_Command(rd, l2capoutbuf, 8,0x01,0x00);
}

void l2cap_connection_response(struct RemoteDevice* rd, uint8_t rxid, uint8_t* dcid, uint8_t* scid, uint8_t result) {
        l2capoutbuf[0] = L2CAP_CMD_CONNECTION_RESPONSE; // Code
        l2capoutbuf[1] = rxid; // Identifier
        l2capoutbuf[2] = 0x08; // Length
        l2capoutbuf[3] = 0x00;
        l2capoutbuf[4] = dcid[0]; // Destination CID
        l2capoutbuf[5] = dcid[1];
        l2capoutbuf[6] = scid[0]; // Source CID
        l2capoutbuf[7] = scid[1];
        l2capoutbuf[8] = result; // Result: Pending or Success
        l2capoutbuf[9] = 0x00;
        l2capoutbuf[10] = 0x00; // No further information
        l2capoutbuf[11] = 0x00;

        L2CAP_Command(rd, l2capoutbuf, 12,0x01,0x00);
}

void l2cap_config_request(struct RemoteDevice* rd,  uint8_t rxid, uint8_t* dcid) {
        l2capoutbuf[0] = L2CAP_CMD_CONFIG_REQUEST; // Code
        l2capoutbuf[1] = rxid; // Identifier
        l2capoutbuf[2] = 0x08; // Length
        l2capoutbuf[3] = 0x00;
        l2capoutbuf[4] = dcid[0]; // Destination CID
        l2capoutbuf[5] = dcid[1];
        l2capoutbuf[6] = 0x00; // Flags
        l2capoutbuf[7] = 0x00;
        l2capoutbuf[8] = 0x01; // Config Opt: type = MTU (Maximum Transmission Unit) - Hint
        l2capoutbuf[9] = 0x02; // Config Opt: length
        l2capoutbuf[10] = 0x00; // MTU
        l2capoutbuf[11] = 0x04;

        L2CAP_Command( rd,l2capoutbuf, 12,0x01,0x00);
}

void l2cap_config_response( struct RemoteDevice* rd, uint8_t rxid, uint8_t* scid) {
        l2capoutbuf[0] = L2CAP_CMD_CONFIG_RESPONSE; // Code
        l2capoutbuf[1] = rxid; // Identifier
        l2capoutbuf[2] = 0x06; // Length
        l2capoutbuf[3] = 0x00;
        l2capoutbuf[4] = scid[0]; // Source CID
        l2capoutbuf[5] = scid[1];
        l2capoutbuf[6] = 0x00; // Flag
        l2capoutbuf[7] = 0x00;
        l2capoutbuf[8] = 0x00; // Result
        l2capoutbuf[9] = 0x00;

        L2CAP_Command( rd,l2capoutbuf, 10,0x01,0x00);
}

void l2cap_disconnection_request( struct RemoteDevice* rd, uint8_t rxid, uint8_t* dcid, uint8_t* scid) {
        l2capoutbuf[0] = L2CAP_CMD_DISCONNECT_REQUEST; // Code
        l2capoutbuf[1] = rxid; // Identifier
        l2capoutbuf[2] = 0x04; // Length
        l2capoutbuf[3] = 0x00;
        l2capoutbuf[4] = dcid[0];
        l2capoutbuf[5] = dcid[1];
        l2capoutbuf[6] = scid[0];
        l2capoutbuf[7] = scid[1];

        L2CAP_Command( rd,l2capoutbuf, 8,0x01,0x00);
}

void l2cap_disconnection_response( struct RemoteDevice* rd, uint8_t rxid, uint8_t* dcid, uint8_t* scid) {
        l2capoutbuf[0] = L2CAP_CMD_DISCONNECT_RESPONSE; // Code
        l2capoutbuf[1] = rxid; // Identifier
        l2capoutbuf[2] = 0x04; // Length
        l2capoutbuf[3] = 0x00;
        l2capoutbuf[4] = dcid[0];
        l2capoutbuf[5] = dcid[1];
        l2capoutbuf[6] = scid[0];
        l2capoutbuf[7] = scid[1];

        L2CAP_Command(rd, l2capoutbuf, 8,0x01,0x00);
}

void l2cap_information_response( struct RemoteDevice* rd, uint8_t rxid, uint8_t infoTypeLow, uint8_t infoTypeHigh) {
        l2capoutbuf[0] = L2CAP_CMD_INFORMATION_RESPONSE; // Code
        l2capoutbuf[1] = rxid; // Identifier
        l2capoutbuf[2] = 0x08; // Length
        l2capoutbuf[3] = 0x00;
        l2capoutbuf[4] = infoTypeLow;
        l2capoutbuf[5] = infoTypeHigh;
        l2capoutbuf[6] = 0x00; // Result = success
        l2capoutbuf[7] = 0x00; // Result = success
        l2capoutbuf[8] = 0x00;
        l2capoutbuf[9] = 0x00;
        l2capoutbuf[10] = 0x00;
        l2capoutbuf[11] = 0x00;

        L2CAP_Command( rd,l2capoutbuf, 12,0x01,0x00);
}





